#ifndef ALGO_RRTSTAR_PRIVATE_H
#define ALGO_RRTSTAR_PRIVATE_H

#include "AlgoRRTStar.h"
#include "algohub_common_type.h"
#include "AlgoEvaluatorDB.h" // Include for DB access
#include <vector>
#include <memory>
#include <random>

namespace algo
{

// Represents a node in the RRT* search tree
struct RRTNode {
    algobase::CoordZ position;
    RRTNode* parent = nullptr;
    double cost = 0.0;

    RRTNode(const algobase::CoordZ& pos) : position(pos) {}
};

// Private implementation class for AlgoRRTStar
class AlgoRRTStarPrivate
{
public:
    AlgoRRTStarPrivate();
    ~AlgoRRTStarPrivate();

    int exec(const algobase::TaskConfig& taskConfig, AlgoEvaluatorDB* db, algobase::Paths& paths);

    // Setters for external data
    void setDepthRange(double min_depth, double max_depth);
    void setCurrentTaskConfig(const algobase::TaskConfig* config) { current_task_config_ = config; }

private:
    // --- Main Logic ---
    algobase::Path runMultiTargetRRTStar(int random_seed, const std::vector<algobase::Polygon>& mandatory_zones);
    bool growTreeTowardsTarget(const algobase::CoordZ& target_pos, const algobase::Polygon* target_zone, RRTNode** connection_node);

    // --- RRT* Core Steps ---
    algobase::CoordZ sampleRandomPoint(const algobase::CoordZ& target_pos);
    RRTNode* findNearestNode(const algobase::CoordZ& point);
    algobase::CoordZ steer(const algobase::CoordZ& from, const algobase::CoordZ& to);
    RRTNode* chooseParent(RRTNode* q_near, const algobase::CoordZ& q_new_pos);
    void rewire(RRTNode* q_new);
    std::vector<RRTNode*> findNeighbors(const algobase::CoordZ& point);

    // --- Path and Environment ---
    algobase::Path reconstructPath(RRTNode* end_node);
    algobase::Path postprocessPath(const algobase::Path& raw_path);
    bool isCollisionFree(const algobase::CoordZ& p1, const algobase::CoordZ& p2) const;
    
    // --- Helpers ---
    double getDepth(const algobase::Coord& coord) const;
    bool isInside(const algobase::Coord& point, const algobase::Polygon& polygon) const;
    double distance(const algobase::CoordZ& p1, const algobase::CoordZ& p2) const;
    algobase::CoordZ getPolygonCenter(const algobase::Polygon& polygon) const;

    // Member Variables
    // Parameters
    int max_iterations_ = 10000;
    double step_size_ = 1000.0; // in meters
    double goal_bias_ = 0.05;
    double search_radius_ = 1500.0;
    double goal_threshold_ = 500.0;
    double min_depth_ = -200.0;
    double max_depth_ = -50.0;

    // Data
    const algobase::TaskConfig* current_task_config_ = nullptr;
    AlgoEvaluatorDB* db_ = nullptr;
    std::unique_ptr<std::mt19937> random_generator_;

    // Tree storage for a single run
    std::vector<std::unique_ptr<RRTNode>> node_storage_;
    std::vector<RRTNode*> node_list_;
};

} // namespace algo

#endif // ALGO_RRTSTAR_PRIVATE_H